#include <stdint.h>

#include "bsp_protocol.hpp"
#include "strutil.hpp"
#include "bsp_uart.h"
#include "log.h"
#include "bsp_hrtimer.h"
#include "stimer.h"
#include <math.h>
#include "iMath.h"
#include "wifi_connect.hpp"
#include "task_main.h"
#include "dcdc.hpp"
#include "board_base.h"

extern class wifiConnect wifiJAP;
#define debug_printf(fmt, ...) UARTx_Printf(&huart3, fmt, ##__VA_ARGS__)
uint8_t SYSSU_Buf[10]; // 解析器的缓存,Split分割后的数据的引索存储到这里
StrUtil SYSSU("SYS:", ',', ";", SYSSU_Buf, sizeof(SYSSU_Buf), NULL, 0);
int sys_print_id = -1;
bsp_timeMesure_t taskOutInfoTime;
uint16_t debug_print_mode = 0;


/*
ADC_IPA_I_HV ADC1IN12  PB1  COMP4 PB0
ADC_IPA_I_LV ADC2IN13 PB12 COMP2 PA7
ADC_HV PC4  ADC2IN5
ADC_HI PC5  ADC2IN11
ADC_LI PB14 ADC2IN14
ADC_LV PB15 ADC2IN15
ADC1_SEQ[T1,T2,T3,IAP_I_HV]
ADC2_SEQ[HV,HI,LI,LV,IAP_I_LV]
*/

kalman1_t HVKF = KALMAN1_INIT(0, 1, 0.1, 10);
kalman1_t HIKF = KALMAN1_INIT(0, 1, 0.1, 50);
kalman1_t LVKF = KALMAN1_INIT(0, 1, 0.1, 10);
kalman1_t LIKF = KALMAN1_INIT(0, 1, 0.1, 50);

void sys_print_info(const void *arg)
{
#if 1
    // bsp_HPMesureTimeStart(&taskOutInfoTime);
    UART_HandleTypeDef *huart = (UART_HandleTypeDef *)arg;
    float T1 = DCDC_GET_T1();
    float T2 = DCDC_GET_T2();
    float T3 = DCDC_GET_T3();
    float HV_V = kalman1_filter(&HVKF, DCDC_GET_HV());
    float HV_I = kalman1_filter(&HIKF, DCDC_GET_HI());
//    float HV_IPA_I = DCDC_GET_IPA_HI();
    float LV_V = kalman1_filter(&LVKF, DCDC_GET_LV());
    float LV_I = kalman1_filter(&LIKF, DCDC_GET_LI());
//    float LV_IPA_I = DCDC_GET_IPA_LI();
    if (debug_print_mode == 0)
    {
        UARTx_Printf(huart, "HVII;LVII:%.1f,%.2f,%.1f,%.1f,%.2f,%.1f,%.2f,%.2f,%.2f\n\r",
                    HV_V, HV_I, HV_V*HV_I,
                    LV_V, LV_I, LV_V*LV_I,
                    T1, T2, T3);
    }
    else if (debug_print_mode == 1)
    {
        UARTx_Printf(huart, "buck(HV_LV_TV_F):%.2f,%.2f,%.2f,%.0f\n\r",
                            HV_V, LV_V, dcpower.V_target, dcpower.F_current);
    }

    bsp_HPMesureTime(&taskOutInfoTime);
#endif
}

void sys_print_info_start(uint16_t invert, UART_HandleTypeDef *huart)
{
    if (sys_print_id == -1)
    {
        sys_print_id = stimer_create_task(sys_print_info, invert, 1, 1);
        if (sys_print_id >= hstimer.size)
        {
            debug_printf("sys_print_info_start error\n\r");
            sys_print_id = -1;
        }
        stimer_task_start(sys_print_id, STIMER_TASK_LOOP, huart);
    }
}

void sys_print_info_stop(void)
{
    stimer_task_set_reserved(sys_print_id, 0);
    stimer_task_stop(sys_print_id);
    sys_print_id = -1;
}

// TODO:做一个命令表，根据命令表自动产生help信息和解析<json>

int SYS_CMD(UART_HandleTypeDef *huart, uint8_t *pdata, uint16_t len)
{
    /*
     * @CMD SYS:Log,level,[level]\n\r
     * @param\n\r
       level:   LOG_LEVEL_NONE=0,\n\r
                LOG_LEVEL_DEBUG=1,\n\r
                LOG_LEVEL_INFO=2,\n\r
                LOG_LEVEL_WARN=3,\n\r
                LOG_LEVEL_ERROR=4,\n\r
                LOG_LEVEL_FATAL=5,\n\r
    */
    uint32_t temp;
    uint8_t ok_flag = 0;
    if (huart == NULL || pdata == NULL || len < 5)
    {
        return 0;
    }
    SYSSU.setSrcAddress(pdata, len);
    if (SYSSU.parse() == 0)
    {
        return 0;
    }
    char *str = SYSSU.getStr(0);
    switch (*str)
    {
    case 'H':
    case 'h':
        if (SYSSU.strCmp(0, "help"))
        {
            temp = 0;
            UARTx_Printf(huart, "@CMD[%d] SYS:Log,help;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:HRTIM,cfg,[CH<A:E>],[freq],[duty<1:100>],[deathTime];\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:HRTIM,pwm,[HV/LV],[0/1];\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:HRTIM,mosL/mosH,[0/1];\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:HRTIM,duty,[HV/LV],(0,100);\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:debug,print,<time>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:debug,format,<0..1>;\n\r");
            UARTx_Printf(huart, "@CMD[%d] SYS:power,on/off,<mode:boost/buck/boostFB/buckFB>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:power,cfg,softStartBuck/softStartBoost,<time>,<initFreq>,<targetFreq>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:power,buckVoltage,<voltage>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:power,SYS:power,pid,buckV,p,<Kp>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:power,SYS:power,pid,buckV,i,<Ki>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:power,SYS:power,pid,buckV,d,<Kd>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:fan,auto,<on/off>;\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:fan,speed,<id:0/1>,[0,1000];\n\r", temp++);
            UARTx_Printf(huart, "@CMD[%d] SYS:debug,DAC,<H/L>,<value>;\n\r", temp++);
            ok_flag = 1;
        }
        if (SYSSU.strCmp(0, "HRTIM"))
        {
            if (SYSSU.strCmp(1, "cfg"))
            {
                bsp_HRTIM_OutConfig((*SYSSU.getData(2) - 'A'), SYSSU.getInt(3), SYSSU.getInt(4));
                bsp_HRTIM_SetDead(*SYSSU.getData(2) - 'A', SYSSU.getInt(5));
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "pwm")) // HRTIM,pwm,HV,0
            {
                if (SYSSU.strCmp(2, "HV"))
                {
                    if (SYSSU.strCmp(3, "0"))
                    {
                        DCDC_HV_PWM_Stop();
                        ok_flag = 1;
                    }
                    else if (SYSSU.strCmp(3, "1"))
                    {
                        DCDC_HV_PWM_Start();
                        ok_flag = 1;
                    }
                }
                else if (SYSSU.strCmp(2, "LV")) // HRTIM,pwm,LV,0
                {
                    if (SYSSU.strCmp(3, "0"))
                    {
                        DCDC_LV_PWM_Stop();
                        ok_flag = 1;
                    }
                    else if (SYSSU.strCmp(3, "1"))
                    {
                        DCDC_LV_PWM_Start();
                        ok_flag = 1;
                    }
                }
            }
            else if (SYSSU.strCmp(1, "duty")) // HRTIM,duty,LV,0
            {

                if (SYSSU.strCmp(2, "HV"))
                {
                    bsp_HRTIM_SetDuty(DCDC_HV_HRTIM_ID, SYSSU.getInt(3));
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "LV"))
                {
                    bsp_HRTIM_SetDuty(DCDC_LV_HRTIM_ID, SYSSU.getInt(3));
                    ok_flag = 1;
                }
            }
            else if (SYSSU.strCmp(1, "mosL")) // HRTIM,MOS,0
            {
                if (SYSSU.strCmp(2, "0"))
                {
                    DCDC_MOSL_DISABLE();
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "1"))
                {
                    DCDC_MOSL_ENABLE();
                    ok_flag = 1;
                }
            }
            else if (SYSSU.strCmp(1, "mosH")) // HRTIM,MOS,0
            {
                if (SYSSU.strCmp(2, "0"))
                {
                    DCDC_MOSH_DISABLE();
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "1"))
                {
                    DCDC_MOSH_ENABLE();
                    ok_flag = 1;
                }
            }
        }
        break;
    case 'p':
        if (SYSSU.strCmp(0, "power")) // SYS:pwoer,on,<mode>
        {
            if (SYSSU.strCmp(1, "pid"))
            {
                if (SYSSU.strCmp(2, "buckV"))
                {
                    switch (SYSSU.getData(3)[0])
                    {
                    case 'p':
                    case 'P'://SYS:power,pid,buckV,p,<Kp>;
                        dcpower.pid_buck_V.Kp = SYSSU.getDouble(4);
                        ok_flag = 1;
                        break;
                    case 'i':
                    case 'I':
                        dcpower.pid_buck_V.Ki = SYSSU.getDouble(4);
                        ok_flag = 1;
                        break;
                    case 'd':
                    case 'D':
                        dcpower.pid_buck_V.Kd = SYSSU.getDouble(4);
                        ok_flag = 1;
                        break;
                    default:
                        break;
                    }
                }
                else if (SYSSU.strCmp(2, "boostV"))
                {

                }
            }
            else if (SYSSU.strCmp(1, "on"))
            {
                if (SYSSU.strCmp(2, "buck"))
                {
                    dcpower.start(dcdcPower::MODE_BUCK_OPEN);
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "boost"))
                {
                    dcpower.start(dcdcPower::MODE_BOOST_OPEN);
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "boostFB"))
                {
                    dcpower.start(dcdcPower::MODE_BOOST_FEEDBACK);
                    ok_flag = 1;
                }
                else if (SYSSU.strCmp(2, "buckFB"))
                {
                    dcpower.start(dcdcPower::MODE_BUCK_FEEDBACK);
                    ok_flag = 1;
                }
            }
            else if (SYSSU.strCmp(1, "off"))
            {
                dcpower.stop();
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "cfg")) // SYS:power,cfg,softStartBuck/softStartBoost,<time>,<initFreq>,<targetFreq>;
            {
                if (SYSSU.strCmp(2, "softStartBuck"))
                {
                    dcpower.soft_start_time = SYSSU.getInt(3);
                    dcpower.soft_start_initFreq_HV = SYSSU.getInt(4);
                    dcpower.soft_start_targetFreq_HV = SYSSU.getInt(5);
                    ok_flag = 1;
                }
                if (SYSSU.strCmp(2, "softStartBoost"))
                {
                    dcpower.soft_start_time = SYSSU.getInt(3);
                    dcpower.soft_start_initFreq_LV = SYSSU.getInt(4);
                    dcpower.soft_start_targetFreq_LV = SYSSU.getInt(5);
                    ok_flag = 1;
                }
            }
            else if (SYSSU.strCmp(1, "buckVoltage")) // SYS:power,buckVoltage,<voltage>;
            {
                dcpower.set_buck_voltage(SYSSU.getInt(2));
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "boostVoltage"))
            {
                //TODO:
            }

        }
        break;
    case 'd': // SYS:debug,print,<time>;
        if (SYSSU.strCmp(0, "debug"))
        {
            if (SYSSU.strCmp(1, "print"))
            {
                if (sys_print_id == -1)
                {
                    sys_print_info_start(SYSSU.getInt(2), huart);
                    ok_flag = 1;
                }
                else
                {
                    sys_print_info_stop();
                    ok_flag = 1;
                }
            }
            else if (SYSSU.strCmp(1, "format"))//SYS:debug,format,<format>;
            {
                debug_print_mode = SYSSU.getInt(2);
                ok_flag = 1;
            } 
            else if (SYSSU.strCmp(1, "buck"))
            {
                dcpower.feedback_buck_start();
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "boost"))
            {
                
                dcpower.feedback_boost_start();
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "stop"))
            {
                HAL_TIM_Base_Stop(&htim15);
                ADC1->CFGR &= ~(1<<12);
            }
            else if (SYSSU.strCmp(1, "DAC")) // SYS:debug,DAC,<H/L>,<value>;
            {
                if (*SYSSU.getData(2) == 'H')
                {
                    DCDC_SR_SET_COMPHV(SYSSU.getInt(3));
                    ok_flag = 1;
                }
                else if (*SYSSU.getData(2) == 'L')
                {
                    DCDC_SR_SET_COMPLV(SYSSU.getInt(3));
                    ok_flag = 1;
                }
            }
                
        }
        break;
    case 'L':
        if (SYSSU.strCmp(0, "Log"))
        {
            switch (SYSSU.getStr(1)[0])
            {
            case 'h':

                break;
            case 'H':
                UARTx_Print(huart, "     * @CMD SYS:Log,level,[level]\n\r     * @param\n\r       level:   LOG_LEVEL_NONE=0,\n\r                LOG_LEVEL_DEBUG=1,\n\r                LOG_LEVEL_INFO=2,\n\r                LOG_LEVEL_WARN=3,\n\r                LOG_LEVEL_ERROR=4,\n\r                LOG_LEVEL_FATAL=5,\n\r");
                ok_flag = 1;
                break;
            case 'l': // set
                temp = SYSSU.getInt(2);
                if (temp < 6)
                {
                    log_set_level(temp);
                    UARTx_Printf(huart, "set log level:%d ok\n\r", temp);
                    ok_flag = 1;
                }
                break;
            default:
                break;
            }
        }
        break;
    case 'w':
        if (SYSSU.strCmp(0, "wifi"))
        {
            if (SYSSU.strCmp(1, "stop")) // SYS:wifi,stop;
            {
                wifiJAP.stop_join_ap();
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "start")) // SYS:wifi,start;
            {
                wifiJAP.start_join_ap("DCDC","00000000");
            }
        }
        break;
    case 'f':
        if (SYSSU.strCmp(0, "fan"))
        {
            if (SYSSU.strCmp(1, "auto")) // SYS:fan,auto,<on/off>;
            {
                SYSSU.getInt(2) ? dcpower.fan_auto_start() : dcpower.fan_auto_stop();
                ok_flag = 1;
            }
            else if (SYSSU.strCmp(1, "speed")) // SYS:fan,speed,<id>,<speed>;
            {
                dcpower.set_fan_speed(SYSSU.getInt(2), SYSSU.getInt(3));
                ok_flag = 1;
            }
        }
		break;
    default:
        break;
    }
    if (ok_flag)
    {
        Beep(50);
        UARTx_Print(huart, "OK\n\r");
    }
    return ok_flag;
}
